// Fill out your copyright notice in the Description page of Project Settings.

#include "MPU6050Test.h"
#include "MPU6050Server.h"

MPU6050Server::MPU6050Server(int nPort, MPU6050Interface* inst) : bQuit(false), mpuInst(inst)
{
	udpSocket.bind(Poco::Net::SocketAddress(8000));
	thread.start(*this);
}

MPU6050Server::~MPU6050Server()
{
	bQuit = true;
	thread.join();
}

void MPU6050Server::DecodeIMUData(unsigned char* chrTemp)
{
	double a[3], w[3], Angle[3], T;
	switch (chrTemp[1])
	{
	case 0x51:
		a[0] = (short(chrTemp[3] << 8 | chrTemp[2])) / 32768.0 * 16;
		a[1] = (short(chrTemp[5] << 8 | chrTemp[4])) / 32768.0 * 16;
		a[2] = (short(chrTemp[7] << 8 | chrTemp[6])) / 32768.0 * 16;
		T = (short(chrTemp[9] << 8 | chrTemp[8])) / 340.0 + 36.25;
		mpuInst->OnAcceleration(a[0], a[1], a[2]);
		break;
	case 0x52:
		w[0] = (short(chrTemp[3] << 8 | chrTemp[2])) / 32768.0 * 2000;
		w[1] = (short(chrTemp[5] << 8 | chrTemp[4])) / 32768.0 * 2000;
		w[2] = (short(chrTemp[7] << 8 | chrTemp[6])) / 32768.0 * 2000;
		T = (short(chrTemp[9] << 8 | chrTemp[8])) / 340.0 + 36.25;
		mpuInst->OnAngularSpeed(w[0], w[1], w[2]);
		break;
	case 0x53:
		Angle[0] = (short(chrTemp[3] << 8 | chrTemp[2])) / 32768.0 * 180;
		Angle[1] = (short(chrTemp[5] << 8 | chrTemp[4])) / 32768.0 * 180;
		Angle[2] = (short(chrTemp[7] << 8 | chrTemp[6])) / 32768.0 * 180;
		T = (short(chrTemp[9] << 8 | chrTemp[8])) / 340.0 + 36.25;
		mpuInst->OnAngle(Angle[0], Angle[1], Angle[2]);
		break;
	}
}

void MPU6050Server::run()
{
	Poco::Timespan ts(250000);

	while (!bQuit)
	{
		if (udpSocket.poll(ts, Poco::Net::Socket::SELECT_READ))
		{
			try
			{
				unsigned char buffer[256];
				Poco::Net::SocketAddress sender;
				int n = udpSocket.receiveFrom(buffer, sizeof(buffer), sender);
				if (n == 11)
				{
					DecodeIMUData(buffer);
				}

			}
			catch (Poco::Exception&)
			{
				
			}
		}
	}
}